import numpy as np
import matplotlib.pyplot as plt

# 生成带噪声的观测数据（真实运动为匀速直线）
np.random.seed(0)
true_x = np.linspace(0, 10, 50)
measurements = true_x + np.random.normal(0, 0.5, size=true_x.shape)

# 卡尔曼滤波参数
A = 1  # 状态转移矩阵
H = 1  # 观测矩阵
Q = 0.01  # 过程噪声协方差
R = 0.5   # 观测噪声协方差

# 初始值
x_est = 0  # 初始状态估计
P = 1      # 初始协方差
x_estimates = []

for z in measurements:
    # 预测
    x_pred = A * x_est
    P_pred = A * P * A + Q
    # 更新
    K = P_pred * H / (H * P_pred * H + R)  # 卡尔曼增益
    x_est = x_pred + K * (z - H * x_pred)
    P = (1 - K * H) * P_pred
    x_estimates.append(x_est)

# 可视化
plt.plot(true_x, label='True Position')
plt.scatter(range(len(measurements)), measurements, color='r', label='Measurements', s=10)
plt.plot(x_estimates, label='Kalman Filter', linestyle='--')
plt.legend()
plt.title('Simple 1D Kalman Filter Example')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.show()

